//   the function for class CBeam, it can be used to creat a assemble of 
//   particles with the twiss parameters
//dt的单位是单位ns。输入的单位是s
//dFreq的单位是Hz
#include "CBeam.h"
#include <fstream>
#include <sstream>
#include <string>
#include <iostream>
#include <iomanip>
#include <cmath>
#include <time.h>
#include <stdlib.h>
#include <public.h>
#include "CLHEP/Matrix/Matrix.h"
#include "CLHEP/Matrix/SymMatrix.h"
using namespace std;
CBeam::CBeam()
{
	alph=new CVector3d();
	beta=new CVector3d();
	emittance=new CVector3d();
	EmitRms=new CVector3d();
	RmsSize=new CVector3d();
	RmsSizep=new CVector3d();
	AveSize=new CVector3d();
	AveSizep=new CVector3d();

}
CBeam::~CBeam()
{
	delete alph;
	delete beta;
	delete emittance;
	delete EmitRms;
	delete RmsSize;
	delete RmsSizep;
	delete AveSize;
	delete AveSizep;
}
void CBeam::setBeam(double ax, double ay, double az, double bx, double by, double bz, double ex, double ey, double ez,
		int n, double cur, double df,double _dt)
//这里应该是全发射度和对应的值
{
	alph->setCVector3d(ax, ay, az);
	beta->setCVector3d(bx, by, bz);
	emittance->setCVector3d(ex, ey, ez);
	particleNumber=n;
	p.resize(n+1);  ///ΪʲôҪ??N+1??Ԫ?أ? 
	current=cur;
	dutyFactor=df;
	dStep=_dt*1e-9;
	for (int i=0; i<=n; i++)
	{
		p.at(i).setPosition(0.0, 0.0, 0.0);  ///at(i)??vector??׼???е????ݣ???ʾ????ָ??λ?ã?i????Ԫ?? ?????ǲ??????? 
		p.at(i).setVelocity(0.0, 0.0, 0.0);
		p.at(i).setE(0.0, 0.0, 0.0);
		p.at(i).setB(0.0, 0.0, 0.0);
		p.at(i).setStep(dStep);
		p.at(0).setLossFlag(0);
		//vector.at(),不是返回的指针吗？这个
	}
}
void CBeam::setStep(double _dt)
{
	dStep=_dt;
	for (int i=0; i<=particleNumber; i++)
		(p.at(i)).setStep(dStep);
}
void CBeam::beamDistribution(string fileName)
{
	ifstream infile(fileName.c_str());
	double x1, x2, x3, x4, x5, x6;  
	stringstream ss;
	string sRow;
	int i=1;
	if(!infile)
	{
		cout<<"can't open the beam distribution file"<<endl;
		return;
	}
	dEnergyAve=0;
	dZAve=0;
	string str1;
	double dInput;
	getline(infile,sRow);
	getline(infile,sRow);
	getline(infile,sRow);
	getline(infile,sRow);
	ss.clear();
	ss.str(sRow);        
	ss>>str1>>str1>>str1>>str1>>dEnergyS;
	dBetaS=dEnergyS/dErest+1.0;
	dBetaS=sqrt(1-1/dBetaS/dBetaS);
	getline(infile,sRow);
	ss.clear();
	ss.str(sRow);        
	ss>>str1>>str1>>str1>>str1>>particleNumber;;
	p.resize(particleNumber+1);  ///ΪʲôҪ??N+1??Ԫ?أ? 
	getline(infile,sRow);
	ss.clear();
	ss.str(sRow);        
	ss>>str1>>str1>>str1>>current;
	getline(infile,sRow);
	ss.clear();
	ss.str(sRow);        
	ss>>str1>>str1>>str1>>dFreq;;
	dFreq=dFreq*1.0e6;
	getline(infile,sRow);
	getline(infile,sRow);
	ss.clear();
	ss.str(sRow);        
	ss>>str1>>str1>>str1>>dutyFactor;
	getline(infile,sRow);
	getline(infile,sRow);

	p.at(0).setVelocity(0,0,dBetaS*clight);
	p.at(0).setPosition(0,0,0);
	p.at(0).setT0(0);
	while(!infile.eof())
	{
		double dvabs;
		getline(infile,sRow);
		if(sRow=="") continue;
		ss.clear();
		ss.str(sRow);        
		ss>>x1>>x2>>x3>>x4>>x5>>x6;
		dEnergyAve+=x6;
		x6=x6/dErest+1.0;
		x6=sqrt(1.0-1.0/x6/x6);    //beta
		x6=clight*(x6/sqrt(1.0+x2*x2+x4*x4));   //vz m/s
		x5=-x5/2.0/pi/dFreq*x6;                 //z in m
		//x5=x5/2.0/pi/dFreq; //z-zs in mz-zs.because the input x5 is phase relative to synchrotron particles//!!!!!problems 
		p.at(i).setT0(0.0);
		//p.at(i).setT0(x5);
		x1=x1+x5*x2*100;          //xt=xz+(0-z)*
		x3=x3+x5*x4*100;          //xt=xz+(0-z)*
		//cout<<x1<<" "<<x3<<endl;
		p.at(i).setPosition(x1/100.0, x3/100.0, x5);  //20120627  //20120724加负号   //20120727
		p.at(i).setVelocity(x2*x6, x4*x6, x6);       //20120727
		dZAve+=p.at(i).getPosition().getZ();
		p.at(i).setLossFlag(0);
		i++;
	}                                         
	dEnergyAve/=particleNumber;
	p.at(0).setLossFlag(0);
	infile.close();
	//把phi的变换，转化成t的变化，以前是转化成z来表示的
}
void CBeam::beamDistribution(int type)
{
	if (type==1)                 
	{
		uniformDis();
	}
	else if (type==2)
	{
		kvDis();
	}
	else if (type==3)
	{
		wbDis();
	}
	else if (type==4)
	{
		DSur();
	}
	else
	{
		cout<<"wrong type ! can not generate the right distibution!"<<endl;
		exit(1);
	}
}
void CBeam::uniformDis()   //三维实空间的均匀分布
{
	double dxm,dym,dzm;
	dxm=sqrt((beta->getX())*(emittance->getX()));
	dym=sqrt((beta->getY())*(emittance->getY()));
	dzm=sqrt((beta->getZ())*(emittance->getZ()));

	double dDxp,dDyp,dDzp;
	double dxp,dyp,dzp;
	double xn,yn,zn;
	double dxpmin,dypmin,dzpmin;
	double dGammax,dGammay,dGammaz;
	dGammax=(1.0+alph->getX()*alph->getX())/beta->getX();
	dGammay=(1.0+alph->getY()*alph->getY())/beta->getY();
	dGammaz=(1.0+alph->getZ()*alph->getZ())/beta->getZ();
	srand( (unsigned)time( NULL ) );   
	double drm=sqrt(dxm*dxm+dym*dym+dzm*dzm);
	for(int i=1;i<particleNumber+1;)
	{
		zn=double(rand()%1001)/1000.0*2*dzm-dzm;
		yn=double(rand()%1001)/1000.0*2*dym-dym;
		xn=double(rand()%1001)/1000.0*2*dxm-dxm;
		//实空间看作正椭圆
		if(yn*yn/dym/dym+zn*zn/dzm/dzm+xn*xn/dxm/dxm<=1.0)
		{

			dDxp=sqrt(dxm*dxm-xn*xn)/beta->getX();
			dDyp=sqrt(dym*dym-yn*yn)/beta->getY();
			dDzp=sqrt(dzm*dzm-zn*zn)/beta->getZ();

			dxpmin=-alph->getX()*xn/beta->getX()-dDxp;
			dypmin=-alph->getY()*yn/beta->getY()-dDyp;
			dzpmin=-alph->getZ()*zn/beta->getZ()-dDzp;

			dxp=dxpmin+2*dDxp*double(rand()%1001)/1000.0;
			dyp=dypmin+2*dDyp*double(rand()%1001)/1000.0;
			dzp=dzpmin+2*dDzp*double(rand()%1001)/1000.0;

      dDxp=double(rand()%1001)/1000.0*2*dGammax*emittance->getX();
			dxp=sqrt(dGammax*emittance->getX()-(dDxp/2.0)*(dDxp/2.0));

			p.at(i).setPosition(xn,yn,zn);
			p.at(i).setVelocity(dxp, dyp, dzp);
			i++;
		}
	}

}
//void CBeam::uniformDis()   //三维实空间的均匀分布
//{
//	double phmax, xmax, ymax;
//	double haPh, dmPh, haX, haY, dmX, dmY;
//	double haE, haTx, haTy;
//	CVector3d gama;
//
//	gama.setX((1.0+alph->getX()*alph->getX())/beta->getX());
//	gama.setY((1.0+alph->getY()*alph->getY())/beta->getY());
//	gama.setZ((1.0+alph->getZ()*alph->getZ())/beta->getZ());
//
//	haPh=pow(emittance->getZ()/gama.getZ(), 0.5);
//	haE=pow(emittance->getZ()*gama.getZ(), 0.5);
//	dmPh=-1.0*haPh*alph->getZ();
//
//	haX=pow(emittance->getX()/gama.getX(), 0.5);
//	haTx=pow(emittance->getX()*gama.getX(), 0.5);
//	dmX=-1.0*alph->getX()*haX;
//
//	haY=pow(emittance->getY()/gama.getY(), 0.5);
//	haTy=pow(emittance->getY()*gama.getY(), 0.5);
//	dmY=-1.0*alph->getY()*haY;
//
//	phmax=pow(haPh*haPh+dmPh*dmPh, 0.5);
//	xmax=pow(haX*haX+dmX*dmX, 0.5);
//	ymax=pow(haY*haY+dmY*dmY, 0.5);
//
//	srand( (unsigned)time( NULL ) );   
//
//	double rxyz, alp, bet, xn, yn, pn;
//	for (int i=1; i<=particleNumber; i++)
//	{
//		rxyz=pow(double(rand()%1000)/1000.0, 0.5);
//
//		alp=double(rand()%1000)/1000.0*pi;
//		bet=double(rand()%1000)/1000.0*2*pi;
//		xn=rxyz*sin(alp*pi/180)*cos(bet*pi/180);
//		yn=rxyz*sin(alp*pi/180)*sin(bet*pi/180);
//		pn=rxyz*cos(alp*pi/180);
//
////  filling ph-W space
////  pn相当于归一化因子
//		
//		double Pnr, E11, E121, E12, E13, E1, E2, Enr;
//
//		Pnr=pn*phmax;
//		E11=haE*dmPh*Pnr;
//		E121=dmPh*dmPh+haPh*haPh-Pnr*Pnr;
//	       // if (E121==0)
//		if (E121<0)
//		{
//			E121=0.0;
//		}
//		E12=haE*haPh*pow(E121, 0.5);
//		E13=dmPh*dmPh+haPh*haPh;
//		E1=(E11+E12)/E13;
//		E2=(E11-E12)/E13;
//		Enr=E2+double(rand()%1000)/1000.0*(E1-E2);
////		p.at(i).position.z=Pnr;
////		p.at(i).velocity.z=Enr;                               pnr, enr: pahse and energy
//
//// filling xx' and yy' space
//		double fact, xm, ym, xnr, ynr, rfx, rfy;
//		double aax, bbx, ccx, aay, bby, ccy;
//		double xp11, xp121, xp12, xp13, xp1, xp2;
//		double yp11, yp121, yp12, yp13, yp1, yp2;
//		double tx, ty;
//
//		fact=sqrt(1-pn*pn);
//		xm=xmax*fact;
//		ym=ymax*fact;
//		xnr=xn*xmax;
//		ynr=yn*ymax;
//
//		rfx=xm/xmax;
//		rfy=ym/ymax;
//		aax=haX*rfx;
//		bbx=haTx*rfx;
//		ccx=dmX*rfx;
//		
//		aay=haY*rfy;
//		bby=haTy*rfy;
//		ccy=dmY*rfy;
//
//		xp11=bbx*ccx*xnr;
//		xp121=ccx*ccx+aax*aax-xnr*xnr;
//		if (xp121<0.0)
//		{
//			xp121=0.0;
//		}
//		xp12=bbx*aax*pow(xp121, 0.5);
//		xp13=ccx*ccx+aax*aax;
//		xp1=(xp11+xp12)/xp13;
//		xp2=(xp11-xp12)/xp13;
//
//		yp11=bby*ccy*ynr;
//		yp121=ccy*ccy+aay*aay-ynr*ynr;
//		if(yp121<0)
//		{
//			yp121=0.0;
//		}
//		yp12=bby*aay*pow(yp121, 0.5);
//		yp13=ccy*ccy+aay*aay;
//		yp1=(yp11+yp12)/yp13;
//		yp2=(yp11-yp12)/yp13;
//
//        tx=xp2+double(rand()%1000)/1000.0*(xp1-xp2);
//		ty=xp2+double(rand()%1000)/1000.0*(yp1-yp2);
//
//// need to transfer from x, x', y, y', phi, de to x, vx, y, vy, z, vz)
//// to be done later
//
////		cout<<xnr<<'\t'<<ynr<<'\t'<<Pnr<<endl;
//		p.at(i).setPosition(xnr, ynr, Pnr);
//		p.at(i).setVelocity(tx, ty, Enr);
//	}
//}

void CBeam::kvDis()
{
	double dxm,dym,dzm,dxpm,dypm,dzpm;
	double dGammax,dGammay,dGammaz;
	dGammax=(1.0+alph->getX()*alph->getX())/beta->getX();
	dGammay=(1.0+alph->getY()*alph->getY())/beta->getY();
	dGammaz=(1.0+alph->getZ()*alph->getZ())/beta->getZ();
	double dPhi2x,dPhi2y,dPhi2z;
	if(alph->getX()==0||dGammax==beta->getX())
	{
		dxm =sqrt((beta->getX())*(emittance->getX()));
		dxpm=sqrt(dGammax*(emittance->getX()));
		dPhi2x=0;
	}
	else
	{
    dPhi2x=atan(alph->getX()*2/(dGammax-beta->getX()));
		if(dPhi2x<0)
		dPhi2x+=pi;
		dxm=(dGammax+beta->getX()+2*alph->getX()*sin(dPhi2x))/2.0/emittance->getX();
		dxpm=(dGammax+beta->getX()-2*alph->getX()*sin(dPhi2x))/2.0/emittance->getX();
	}
	if(alph->getY()==0||dGammay==beta->getY())
	{
		dym =sqrt((beta->getY())*(emittance->getY()));
		dypm=sqrt(dGammay*(emittance->getY()));
		dPhi2y=0;
	}
	else
	{
    dPhi2y=atan(alph->getY()*2/(dGammay-beta->getY()));
		if(dPhi2y<0)
		dPhi2y+=pi;
		dym=(dGammay+beta->getY()+2*alph->getY()*sin(dPhi2y))/2.0/emittance->getY();
		dypm=(dGammay+beta->getY()-2*alph->getY()*sin(dPhi2y))/2.0/emittance->getY();
	}
	if(alph->getZ()==0||dGammaz==beta->getZ())
	{
		dzm =sqrt((beta->getZ())*(emittance->getZ()));
		dzpm=sqrt(dGammaz*(emittance->getZ()));
		dPhi2z=0;
	}
	else
	{
    dPhi2z=atan(alph->getZ()*2/(dGammaz-beta->getZ()));
		if(dPhi2z<0)
		dPhi2z+=pi;
		dzm=(dGammaz+beta->getZ()+2*alph->getZ()*sin(dPhi2z))/2.0/emittance->getZ();
		dzpm=(dGammaz+beta->getZ()-2*alph->getZ()*sin(dPhi2z))/2.0/emittance->getZ();
	}
	double xn,yn,zn,xpn,ypn,zpn;
	double xn1,yn1,ypn1,xpn1,zn1,zpn1;
	srand( (unsigned)time( NULL ) );   
	for(int i=1;i<particleNumber+1;)
	{
		do{
			xn=  double(rand()%1000)/1000.0*2.0-1.0;
			yn=  double(rand()%1000)/1000.0*2.0-1.0;
			xpn= double(rand()%1000)/1000.0*2.0-1.0;
			ypn= double(rand()%1000)/1000.0*2.0-1.0;
		}
		while(xn*xn+yn*yn+ypn*ypn+xpn*xpn>1.0);
		double dr=sqrt(xn*xn+yn*yn+ypn*ypn+xpn*xpn);
		xn= dxm*xn/dr;
		yn= dym*yn/dr;
		xpn= dxpm*xpn/dr;
		ypn= dypm*ypn/dr;
		do{
		zn= (double(rand()%1000)/1000.0*2.0-1.0);
		zpn=(double(rand()%1000)/1000.0*2.0-1.0);
		}while(zn*zn+zpn*zpn>1.0);
		dr=sqrt(zn*zn+zpn*zpn);
		zn= dzm*zn/dr;
		zpn=dzpm*zpn/dr;

		xn1 =xn*cos(dPhi2x/2.0)-xpn*sin(dPhi2x/2.0);
		xpn1=xn*sin(dPhi2x/2.0)+xpn*cos(dPhi2x/2.0);
		yn1 =yn*cos(dPhi2y/2.0)-ypn*sin(dPhi2y/2.0);
		ypn1=yn*sin(dPhi2y/2.0)+ypn*cos(dPhi2y/2.0);
		zn1 =zn*cos(dPhi2z/2.0)-zpn*sin(dPhi2z/2.0);
		zpn1=zn*sin(dPhi2z/2.0)+zpn*cos(dPhi2z/2.0);
		p.at(i).setPosition(xn1,yn1,zn1);
		p.at(i).setVelocity(xpn1, ypn1, zpn1);
		i++;
	}
}

void CBeam::DSur()
{
	double dxm,dym,dzm,dxpm,dypm,dzpm;
	double dPhi1,dPhi2,dPhi3;
	double dGammax,dGammay,dGammaz;
	dGammax=(1.0+alph->getX()*alph->getX())/beta->getX();
	dGammay=(1.0+alph->getY()*alph->getY())/beta->getY();
	dGammaz=(1.0+alph->getZ()*alph->getZ())/beta->getZ();

	dPhi1=atan(2*alph->getX()/(dGammax-beta->getX()))/2.0;
	dPhi2=atan(2*alph->getY()/(dGammay-beta->getY()))/2.0;
	dPhi3=atan(2*alph->getZ()/(dGammaz-beta->getZ()))/2.0;

	dxm= 1.0/sqrt(cos(2*dPhi1)/(sin(dPhi1)*sin(dPhi1)*dGammax-beta->getX()*cos(dPhi1)*cos(dPhi1)));
	dxm= 1.0/sqrt(cos(2*dPhi1)/(sin(dPhi1)*sin(dPhi1)*dGammax-beta->getX()*cos(dPhi1)*cos(dPhi1)));
	dxm= 1.0/sqrt(cos(2*dPhi1)/(sin(dPhi1)*sin(dPhi1)*dGammax-beta->getX()*cos(dPhi1)*cos(dPhi1)));
	dxm= 1.0/sqrt((beta->getX()+dGammax)/2.0/emittance->getX()+alph->getX()/emittance->getX()/sin(2*dPhi1));
	dym= 1.0/sqrt((beta->getY()+dGammay)/2.0/emittance->getY()+alph->getY()/emittance->getY()/sin(2*dPhi2));
	dzm= 1.0/sqrt((beta->getZ()+dGammaz)/2.0/emittance->getZ()+alph->getZ()/emittance->getZ()/sin(2*dPhi3));
	dxpm=1.0/sqrt((beta->getX()+dGammax)/2.0/emittance->getX()-alph->getX()/emittance->getX()/sin(2*dPhi1));
	dypm=1.0/sqrt((beta->getY()+dGammay)/2.0/emittance->getY()-alph->getY()/emittance->getY()/sin(2*dPhi2));
	dzpm=1.0/sqrt((beta->getZ()+dGammaz)/2.0/emittance->getZ()-alph->getZ()/emittance->getZ()/sin(2*dPhi3));
	//dxpm=sqrt(dGammax*emittance->getX());
	//dypm=sqrt(dGammay*emittance->getY());
	//dzpm=sqrt(dGammaz*emittance->getZ());
	double xn,yn,zn,xpn,ypn,zpn;
	double dDzp,dzpmin;
	double dr=sqrt(dxm*dxm+dym*dym+dxpm*dxpm+dypm*dypm);
	double dfx,dfy,dfz;
	srand( (unsigned)time( NULL ) );   
  double dPhi,dTheta;
	for(int i=1;i<particleNumber+1;)
	{
	/*
		//method 1  for distribution on 3d sphere surface
    dTheta=double(rand()%1001)/1000.0*2-1;
    dPhi=double(rand()%1001)/1000.0*2*pi;
    dTheta=acos(dTheta);
		dfx=double(rand()%1001)/1000.0;
		dfx=pow(3*dfx,1.0/3.0);
    xn=dfx*sin(dTheta)*cos(dPhi);
    yn=dfx*sin(dTheta)*sin(dPhi);
    zn=dfx*cos(dTheta);
		p.at(i).setPosition(xn,yn,zn);
		p.at(i).setVelocity(dTheta,dPhi,0);
		i++;
		*/
		/*
		//method 2  for distribution on 3d sphere surface
		xn=double(rand()%1001)/1000.0;
		yn=double(rand()%1001)/1000.0;
		zn=double(rand()%1001)/1000.0;
		xn=xn/sqrt(xn*xn+yn*yn+zn*zn);
		yn=yn/sqrt(xn*xn+yn*yn+zn*zn);
		zn=zn/sqrt(xn*xn+yn*yn+zn*zn);
		p.at(i).setPosition(xn,yn,zn);
		p.at(i).setVelocity(0,0,0);
		i++;
		*/
    /*
		do
		{
			xn= (double(rand()%1000)/1000.0*2.0-1.0)*dxm;
			yn= (double(rand()%1000)/1000.0*2.0-1.0)*dym;
			xpn=(double(rand()%1000)/1000.0*2.0-1.0)*dxpm;
			ypn=(double(rand()%1000)/1000.0*2.0-1.0)*dypm;
		}while(xn*xn/dxm/dxm+xpn*xpn/dxpm/dxpm>1.0||yn*yn/dym/dym+ypn*ypn/dypm/dypm>1.0||xn*xn/dxm/dxm+xpn*xpn/dxpm/dxpm+yn*yn/dym/dym+ypn*ypn/dypm/dypm>1.0);
		xn=xn*cos(dPhi1)+xpn*sin(dPhi1);
		yn=yn*cos(dPhi2)+ypn*sin(dPhi2);
		xpn=xn*sin(dPhi1)+xpn*cos(dPhi1);
		ypn=yn*sin(dPhi2)+ypn*cos(dPhi2);
		zn= (double(rand()%1000)/1000.0*2.0-1.0)*dzm;
		zpn=(double(rand()%1000)/1000.0*2.0-1.0)*dzpm;
		p.at(i).setPosition(xn,yn,zn);
		p.at(i).setVelocity(xpn,ypn,zpn);
		i++;
    */
    double a=40,b=3,c=5,d=6;
		//double dAlph=double(rand()%1000)/1000.0*2.0*pi;
		//dTheta=(double(rand()%1000)/1000.0*2.0-1.0);
		//dTheta=acos(dTheta);
		//double fr=double(rand()%1000)/1000.0;
		//double r=a*pow(fr,1.0/4.0);
		//xn=r*sin(dTheta)*cos(dPhi);
		//yn=b/a*r*sin(dTheta)*sin(dPhi);
		//zn=c/a*r*cos(dTheta);
		xpn=0;
		double yn1;
		double xn1;
		double dP2=pi/6.0;
		do{
		xn1= double(rand()%1000)/1000.0*2.0-1;
		yn1= double(rand()%1000)/1000.0*2.0-1;
		xn=xn1*cos(dP2)+yn1*sin(dP2);
		yn=xn1*sin(dP2)-yn1*cos(dP2);
		zn= double(rand()%1000)/1000.0*2.0-1;
		xpn= double(rand()%1000)/1000.0*2.0-1;
		}
		while(xn*xn+yn*yn+zn*zn+xpn*xpn>1.0);
		double dr=sqrt(xn*xn+yn*yn+zn*zn+xpn*xpn);
		yn= a*yn/dr;
		xn= b*xn/dr;
		zn= c*zn/dr;
		xpn= d*xpn/dr;
		//reverse circle dP2;
		p.at(i).setPosition(xn,yn,zn);
		p.at(i).setVelocity(xpn,0,0);
		i++;
    
		//	dfx=double(rand()%1001)/1000.0*pi/2.0;
		//	xn=double(rand()%1001)/1000.0*2-1.0;
		//}
		//while(dfx>pi/2.0*sqrt(1.0-xn*xn));
		//do
		//{
		//	dfx=double(rand()%1001)/1000.0*pi/2.0;
		//	yn=double(rand()%1001)/1000.0*2.0-1.0;
		//}
		//while(dfx>=pi/2.0*sqrt(1.0-yn*yn)||yn*yn+xn*xn>1.0);
		//zn=sqrt(1.0-xn*xn-yn*yn);
		//flag=rand()%10;
		//if(flag<5) zn=zn*(-1);
		//else if(flag>5) ;
		//dfx=double(rand()%1001)/1000.0*pi/2.0;
		//if(dfx<pi/2.0*sqrt(1.0-zn*zn))
		//{
		//		p.at(i).setPosition(xn,yn,zn);
		//		p.at(i).setVelocity(0,0,0);
		//		i++;
		//}
				//dfx=double(rand()%1001)/1000.0*pi/2.0;
				//zn=double(rand()%1001)/1000.0*2-1.0;
				//if(dfx<pi/2.0*sqrt(1.0-zn*zn)&&zn<=sqrt(1.0-xn*xn-yn*yn))
				//{
				//p.at(i).setPosition(xn,yn,zn);
				//p.at(i).setVelocity(0,0,0);
				//i++;
				//}
	}
}
void CBeam::wbDis()
{
  double dxm,dym,dzm,dxpm,dypm,dzpm;
	dxm=sqrt((beta->getX())*(emittance->getX()));
	dym=sqrt((beta->getY())*(emittance->getY()));
	dzm=sqrt((beta->getZ())*(emittance->getZ()));
	double dGammax,dGammay,dGammaz;
	dGammax=(1.0+alph->getX()*alph->getX())/beta->getX();
	dGammay=(1.0+alph->getY()*alph->getY())/beta->getY();
	dGammaz=(1.0+alph->getZ()*alph->getZ())/beta->getZ();
	dxpm=sqrt(dGammax*emittance->getX());
	dypm=sqrt(dGammay*emittance->getY());
	dzpm=sqrt(dGammaz*emittance->getZ());
	double xn,yn,zn,xpn,ypn,zpn;
	double dDzp,dzpmin;
	double dr=sqrt(dxm*dxm+dym*dym+dxpm*dxpm+dypm*dypm);
	srand( (unsigned)time( NULL ) );   
	for(int i=1;i<particleNumber+1;)
	{
	  yn=double(rand()%1001)/1000.0*2*dym-dym;
	  xn=double(rand()%1001)/1000.0*2*dxm-dxm;
	  zn=double(rand()%1001)/1000.0*2*dzm-dzm;
	  ypn=double(rand()%1001)/1000.0*2*dypm-dypm;
	  xpn=double(rand()%1001)/1000.0*2*dxpm-dxpm;
	  zpn=double(rand()%1001)/1000.0*2*dzpm-dzpm;
		//实空间看作正椭圆
		if(yn*yn/dym/dym+ypn*ypn/dypm/dypm+xpn*xpn/dxpm/dxpm+xn*xn/dxm/dxm+zpn*zpn/dzpm/dzpm+zn*zn/dzm/dzm<=1.0)
		{
			p.at(i).setPosition(xn,yn,zn);
			p.at(i).setVelocity(xpn, ypn, zpn);
			i++;
		}
	}

}
void CBeam::calRms_lab(int _diagflag)
{
  diagflag=_diagflag;
	dBetaS=p.at(0).getVelocity().getZ()/clight;          //有问题，另外两个关于设置betas和energys的函数被架空了
	double dGammaS=1.0/sqrt(1.0-dBetaS*dBetaS);
	dEnergyS=dErest*(dGammaS-1.0);
	double dZS=p.at(0).getPosition().getZ();
	double dVzTmp,dETmp;     //particle energy versus EnergyS
	//平均值                     
	double dZTmp,dPhiTmp,dVxTmp,dVyTmp,dXTmp,dYTmp,dRTmp;
	double dPxTmp,dPyTmp,dPzTmp;


	vector<double> vdETmp;    		      
	vdETmp.resize(0);
	vdETmp.push_back(0);
	vector<double> vdPhiTmp;    		      
	vdPhiTmp.resize(0);
	vdPhiTmp.push_back(0);

	dXMax=fabs(p.at(1).getPosition().getX());
	dYMax=fabs(p.at(1).getPosition().getY());
	dZMax=fabs(p.at(1).getPosition().getZ());
	dZMin=fabs(p.at(1).getPosition().getZ());
	dRMax=0.0;
	dXAve=0;dYAve=0;dZAve=0;
	dXpAve=0;dYpAve=0;dZpAve=0;
	dPxAve=0;dPyAve=0;dPzAve=0;
	dPhiAve=0;
	dETmp=0;
	dEnergyAve=0;
	dBetaAve=0;
	dBetaZAve=0;
	int lossFlag;
	GoodParticle=0;ParticleLossTran=0;ParticleLossLong=0;
	//束团中心纵向位置
	for(int iTmp=1;iTmp<=particleNumber;iTmp++)
	{
		lossFlag=p.at(iTmp).getLossFlag();
		if(lossFlag==1) { ParticleLossTran+=1;}
		else if(lossFlag==-1) {ParticleLossLong+=1;}
		else if(lossFlag==0)
		{
			dVzTmp=p.at(iTmp).getVelocity().getZ();
			dBetaZAve+=(dVzTmp/clight);
			dZTmp=p.at(iTmp).getPosition().getZ();
			dZAve+=dZTmp;//
			GoodParticle+=1;
		}
	}
	// cal the current according to the particle Number
	current=double(GoodParticle)/double(particleNumber)*current;
	dBetaZAve/=GoodParticle;
	dZAve/=GoodParticle;
	for(int iTmp=1;iTmp<=particleNumber;iTmp++)
	{
		lossFlag=p.at(iTmp).getLossFlag();
		//主义下面几行的位置，不能写在if语句内
		dVxTmp=p.at(iTmp).getVelocity().getX();
		dVyTmp=p.at(iTmp).getVelocity().getY();
		dVzTmp=p.at(iTmp).getVelocity().getZ();
		dETmp=dErest/sqrt(1.0-(dVzTmp*dVzTmp+dVxTmp*dVxTmp+dVyTmp*dVyTmp)/clight/clight)-dErest;
		vdETmp.push_back(dETmp);
		if(lossFlag==0)
		{
			dXTmp=p.at(iTmp).getPosition().getX();
			dYTmp=p.at(iTmp).getPosition().getY();
			dZTmp=p.at(iTmp).getPosition().getZ();
			dBetaAve+=sqrt(dVzTmp*dVzTmp+dVxTmp*dVxTmp+dVyTmp*dVyTmp)/clight;
		  dPxTmp=dVxTmp*dErest*1.0e6/clight/clight*(dETmp/dErest+1);
		  dPyTmp=dVyTmp*dErest*1.0e6/clight/clight*(dETmp/dErest+1);
		  dPzTmp=dVzTmp*dErest*1.0e6/clight/clight*(dETmp/dErest+1);
			p.at(iTmp).setVelocity(dPxTmp,dPyTmp,dPzTmp);
			dXTmp=dXTmp+dVxTmp*(dZAve-dZTmp)/dVzTmp*diagflag;
			dYTmp=dYTmp+dVyTmp*(dZAve-dZTmp)/dVzTmp*diagflag;
			//p.at(iTmp).setPosition(dXTmp,dYTmp,dZTmp);
			if(iTmp==1) { dWmin=dETmp;dWmax=dETmp; }
			if(iTmp!=1) 
			{ 
			  if(dETmp<dWmin) dWmin=dETmp;
				if(dETmp>dWmax) dWmax=dETmp;
			}
			dRTmp=sqrt(dXTmp*dXTmp+dYTmp*dYTmp);
			if(fabs(dXTmp)>dXMax)  dXMax=fabs(dXTmp);
			if(fabs(dYTmp)>dYMax)  dYMax=fabs(dYTmp);
			if(dZTmp>dZMax)  dZMax=dZTmp;
			if(dZTmp<dZMin)  dZMin=dZTmp;
			if(dRMax<dRTmp)   dRMax=dRTmp;
			dXAve+=dXTmp;
			dYAve+=dYTmp;
			dRAve+=dRTmp;

			dPxAve+=dPxTmp;
			dPyAve+=dPyTmp;
			dPzAve+=dPzTmp;
			dEnergyAve+=dETmp;
		}
	}
	dXAve/=GoodParticle;
	dYAve/=GoodParticle;
	dRAve/=GoodParticle;
	dPxAve/=GoodParticle;
	dPyAve/=GoodParticle;
	dPzAve/=GoodParticle;
	dEnergyAve=dEnergyAve/GoodParticle;                
	dBetaAve/=GoodParticle;

	//rms值
	dXRms=0;dYRms=0;dZRms=0;dPhiRms=0;
	dXpRms=0;dYpRms=0;dZpRms=0;
	dPxRms=0;dPyRms=0;dPzRms=0;
	dERms=0;
	dXXp=0;dYYp=0;dZZp=0;
	dXPx=0;dYPy=0;dZPz=0;
	double dPhiW=0;
	//for 4D emit
	double dXY,dXPy,dPxPy,dPxY;
	dXY=0;dXPy=0;dXPy=0;dPxPy=0;
	//for 6D emit 
	double dXZ,dXPz,dPxZ,dPxPz,dYZ,dYPz,dPyZ,dPyPz;
	dXZ=0;dXPz=0;dPxZ=0;dPxPz=0;dYZ=0;dYPz=0;dPyZ=0;dPyPz=0;
	
	for(int iTmp=1;iTmp<=particleNumber;iTmp++)
	{
		lossFlag=p.at(iTmp).getLossFlag();
		if(lossFlag==0)
		{
			dXRms+=pow(p.at(iTmp).getPosition().getX()-dXAve,2);
			dYRms+=pow(p.at(iTmp).getPosition().getY()-dYAve,2);
			dZRms+=pow(p.at(iTmp).getPosition().getZ()-dZAve,2);    //20120709
			dPxRms+=pow(p.at(iTmp).getVelocity().getX()-dPxAve,2);
			dPyRms+=pow(p.at(iTmp).getVelocity().getY()-dPyAve,2);
			dPzRms+=pow(p.at(iTmp).getVelocity().getZ()-dPzAve,2);
		//dZpRms+=pow(p.at(iTmp).getVelocity().getZ()-dEnergyAve,2);
			dERms+=pow(vdETmp[iTmp]-dEnergyAve,2); 
			dXPx+=(p.at(iTmp).getPosition().getX()-dXAve)*(p.at(iTmp).getVelocity().getX()-dPxAve);
			dYPy+=(p.at(iTmp).getPosition().getY()-dYAve)*(p.at(iTmp).getVelocity().getY()-dPyAve);
			dZPz+=(p.at(iTmp).getPosition().getZ()-dZAve)*(p.at(iTmp).getVelocity().getZ()-dPzAve);  //20120726
			//for 4D emit
			dXY+=(p.at(iTmp).getPosition().getX()-dXAve)*(p.at(iTmp).getPosition().getY()-dYAve);
			dXPy+=(p.at(iTmp).getPosition().getX()-dXAve)*(p.at(iTmp).getVelocity().getY()-dPyAve);
			dPxY+=(p.at(iTmp).getVelocity().getX()-dPxAve)*(p.at(iTmp).getPosition().getY()-dYAve);
			dPxPy+=(p.at(iTmp).getVelocity().getX()-dPxAve)*(p.at(iTmp).getVelocity().getY()-dPyAve);
			//for 6D emit
			dXZ+=(p.at(iTmp).getPosition().getX()-dXAve)*(p.at(iTmp).getPosition().getZ()-dZAve);
			dXPz+=(p.at(iTmp).getPosition().getX()-dXAve)*(p.at(iTmp).getVelocity().getZ()-dPzAve);
			dPxZ+=(p.at(iTmp).getVelocity().getX()-dPxAve)*(p.at(iTmp).getPosition().getZ()-dZAve);
			dPxPz+=(p.at(iTmp).getVelocity().getX()-dPxAve)*(p.at(iTmp).getVelocity().getZ()-dPzAve);

			dYZ+=(p.at(iTmp).getPosition().getY()-dYAve)*(p.at(iTmp).getPosition().getZ()-dZAve);
			dYPz+=(p.at(iTmp).getPosition().getY()-dYAve)*(p.at(iTmp).getVelocity().getZ()-dPzAve);
			dPyZ+=(p.at(iTmp).getVelocity().getY()-dPyAve)*(p.at(iTmp).getPosition().getZ()-dZAve);
			dPyPz+=(p.at(iTmp).getVelocity().getY()-dPyAve)*(p.at(iTmp).getVelocity().getZ()-dPzAve);

		}
	}
	dXRms=sqrt(dXRms/GoodParticle);
	dYRms=sqrt(dYRms/GoodParticle);
	dZRms=sqrt(dZRms/GoodParticle);
	dPxRms=sqrt(dPxRms/GoodParticle);
	dPyRms=sqrt(dPyRms/GoodParticle);
	dPzRms=sqrt(dPzRms/GoodParticle);
	dERms=sqrt(dERms/GoodParticle);
	dXPx/=GoodParticle;
	dYPy/=GoodParticle;
	dZPz/=GoodParticle;
	//for 4D emit
	dXY/=GoodParticle;
	dPxY/=GoodParticle;
	dXPy/=GoodParticle;
	dPxPy/=GoodParticle;
	//for 6D emit
	dXZ/=GoodParticle;
	dPxZ/=GoodParticle;
	dXPz/=GoodParticle;
	dPxPz/=GoodParticle;

	dYZ/=GoodParticle;
	dPyZ/=GoodParticle;
	dYPz/=GoodParticle;
	dPyPz/=GoodParticle;
	//要计算的2D发射度xxp，yyp，phiW，zzp，zDelt和相应的alph、beta值
	//  Vector3d计算的是xxp，yyp，zzp，单位是cm＊mrad

	double dEmitRmsX,dEmitRmsY,dEmitRmsZ;
	double dBetaRmsX,dBetaRmsY,dBetaRmsZ;
	double dAlphRmsX,dAlphRmsY,dAlphRmsZ;
	dEmitRmsX=pow(dXRms*dXRms*dPxRms*dPxRms-dXPx*dXPx,0.5);
	dEmitRmsY=pow(dYRms*dYRms*dPyRms*dPyRms-dYPy*dYPy,0.5);
	dEmitRmsZ=pow(dZRms*dZRms*dPzRms*dPzRms-dZPz*dZPz,0.5);   //

	CLHEP::HepSymMatrix *matrix_emit=new CLHEP::HepSymMatrix(6,1);

	(*matrix_emit)[0][0]=  dXRms*dXRms*1.0e5;
	(*matrix_emit)[0][1]=         dXPx*1.0e5;
	(*matrix_emit)[0][2]=          dXY*1.0e5;
	(*matrix_emit)[0][3]=         dXPy*1.0e5;

	(*matrix_emit)[1][1]=dPxRms*dPxRms*1.0e5;
	(*matrix_emit)[1][2]=         dPxY*1.0e5;
	(*matrix_emit)[1][3]=        dPxPy*1.0e5;

	(*matrix_emit)[2][2]=  dYRms*dYRms*1.0e5;
	(*matrix_emit)[2][3]=         dYPy*1.0e5;

	(*matrix_emit)[3][3]=dPyRms*dPyRms*1.0e5;

  dEmit4D=matrix_emit->determinant();
	dEmit4D=sqrt(dEmit4D); //nunormalized

  //for emit 6D
	(*matrix_emit)[0][4]=  dXZ*1.0e5;
	(*matrix_emit)[0][5]= dXPz*1.0e5;
	(*matrix_emit)[1][4]= dPxZ*1.0e5;
	(*matrix_emit)[1][5]=dPxPz*1.0e5;
	(*matrix_emit)[2][4]=  dYZ*1.0e5;
	(*matrix_emit)[2][5]= dYPz*1.0e5;
	(*matrix_emit)[3][4]= dPyZ*1.0e5;
	(*matrix_emit)[3][5]=dPyPz*1.0e5;
	
	(*matrix_emit)[4][4]=  dZRms*dZRms*1.0e5;
	(*matrix_emit)[4][5]=         dZPz*1.0e5;
	(*matrix_emit)[5][5]=dPzRms*dPzRms*1.0e5;

  dEmit6D=matrix_emit->determinant();
	dEmit6D=sqrt(dEmit6D);

	dAlphRmsX=-dXPx/dEmitRmsX;                   
	dAlphRmsY=-dYPy/dEmitRmsY;
	dAlphRmsZ=-dZPz/dEmitRmsZ;
	dBetaRmsX=dXRms*dXRms/dEmitRmsX;     //cm/mrad  with unnormalized emittance
	dBetaRmsY=dYRms*dYRms/dEmitRmsY;     //cm/mrad
	dBetaRmsZ=dZRms*dZRms/dEmitRmsZ;           //cm/mrad

//emit/mc*1.0e5
	dEmitRmsX/=(dErest*10.0/clight);                 //or dPAve?
	dEmitRmsY/=(dErest*10.0/clight);
	dEmitRmsZ/=(dErest*10.0/clight);

  double dGx,dGy,dGz;
	dGx=(1.0+dAlphRmsX*dAlphRmsX)/dBetaRmsX;
	dGy=(1.0+dAlphRmsY*dAlphRmsY)/dBetaRmsY;
	dGz=(1.0+dAlphRmsZ*dAlphRmsZ)/dBetaRmsZ;
  dXpRms=sqrt(dGx*dEmitRmsX);
  dYpRms=sqrt(dGy*dEmitRmsY);
  dZpRms=sqrt(dGz*dEmitRmsZ);

	//dEmitRmsX*=(dBetaAve*(dEnergyAve/dErest+1.0));       //cm*mrad
	//dEmitRmsY*=(dBetaAve*(dEnergyAve/dErest+1.0));       //cm*mrad
	//dEmitRmsZ*=(dBetaAve*pow((dEnergyAve/dErest+1.0),3));       //cm*mrad
	EmitRms->setCVector3d(dEmitRmsX,dEmitRmsY,dEmitRmsZ); 

	RmsSize->setCVector3d(dXRms,dYRms,dZRms);
	//RmsSizep->setCVector3d(dXpRms,dYpRms,dZpRms);
	RmsSizep->setCVector3d(dPxRms,dPyRms,dPzRms);
	AveSize->setCVector3d(dXAve,dYAve,dZAve);
	AveSizep->setCVector3d(dPxAve,dPyAve,dPzAve);
	alph->setCVector3d(dAlphRmsX,dAlphRmsY,dAlphRmsZ);
	beta->setCVector3d(dBetaRmsX,dBetaRmsY,dBetaRmsZ);

	//----change coordinates to original
	for(int iTmp=1;iTmp<=particleNumber;iTmp++)
	{
		lossFlag=p.at(iTmp).getLossFlag();
		if(lossFlag==0)
		{
			dETmp=vdETmp[iTmp];
			dVxTmp=p.at(iTmp).getVelocity().getX()/dErest/1.0e6*clight*clight/(dETmp/dErest+1);
			dVyTmp=p.at(iTmp).getVelocity().getY()/dErest/1.0e6*clight*clight/(dETmp/dErest+1);
			dVzTmp=p.at(iTmp).getVelocity().getZ()/dErest/1.0e6*clight*clight/(dETmp/dErest+1);
			p.at(iTmp).setVelocity(dVxTmp,dVyTmp,dVzTmp);
			dXTmp=p.at(iTmp).getPosition().getX();
			dYTmp=p.at(iTmp).getPosition().getY();
			dZTmp=p.at(iTmp).getPosition().getZ();
			dXTmp=dXTmp-dVxTmp*(dZAve-dZTmp)/dVzTmp*diagflag;
			dYTmp=dYTmp-dVyTmp*(dZAve-dZTmp)/dVzTmp*diagflag;
			//p.at(iTmp).setPosition(dXTmp,dYTmp,dZTmp);
		}
	}
	delete matrix_emit;
}
void CBeam::calRms(int _diagflag)
{
  diagflag=_diagflag;
	dBetaS=p.at(0).getVelocity().getZ()/clight;          //有问题，另外两个关于设置betas和energys的函数被架空了
	double dGammaS=1.0/sqrt(1.0-dBetaS*dBetaS);
	dEnergyS=dErest*(dGammaS-1.0);
	double dZS=p.at(0).getPosition().getZ();
	double dVzTmp,dETmp;     //particle energy versus EnergyS
	//平均值                     
	double dZTmp,dPhiTmp,dVxTmp,dVyTmp,dXTmp,dYTmp,dRTmp;
	double dPxTmp,dPyTmp,dPzTmp;


	vector<double> vdETmp;    		      
	vdETmp.resize(0);
	vdETmp.push_back(0);
	vector<double> vdPhiTmp;    		      
	vdPhiTmp.resize(0);
	vdPhiTmp.push_back(0);

	dXMax=fabs(p.at(1).getPosition().getX());
	dYMax=fabs(p.at(1).getPosition().getY());
	dZMax=fabs(p.at(1).getPosition().getZ());
	dZMin=fabs(p.at(1).getPosition().getZ());
	dRMax=0.0;
	dXAve=0;dYAve=0;dZAve=0;
	dXpAve=0;dYpAve=0;dZpAve=0;
	dPxAve=0;dPyAve=0;dPzAve=0;
	dPhiAve=0;
	dETmp=0;
	dEnergyAve=0;
	dBetaAve=0;
	dBetaZAve=0;
	int lossFlag;
	GoodParticle=0;ParticleLossTran=0;ParticleLossLong=0;
	for(int iTmp=1;iTmp<=particleNumber;iTmp++)
	{
		lossFlag=p.at(iTmp).getLossFlag();
		if(lossFlag==1) { ParticleLossTran+=1;}
		else if(lossFlag==-1) {ParticleLossLong+=1;}
		else if(lossFlag==0)
		{
			dVzTmp=p.at(iTmp).getVelocity().getZ();
			dBetaZAve+=(dVzTmp/clight);
			dZTmp=p.at(iTmp).getPosition().getZ();
			dZAve+=dZTmp;//
			GoodParticle+=1;
		}
	}
	current=double(GoodParticle)/double(particleNumber)*current;
	dBetaZAve/=GoodParticle;
	dZAve/=GoodParticle;
	for(int iTmp=1;iTmp<=particleNumber;iTmp++)
	{
		lossFlag=p.at(iTmp).getLossFlag();
		//主义下面几行的位置，不能写在if语句内
		dVxTmp=p.at(iTmp).getVelocity().getX();
		dVyTmp=p.at(iTmp).getVelocity().getY();
		dVzTmp=p.at(iTmp).getVelocity().getZ();
		dETmp=dErest/sqrt(1.0-(dVzTmp*dVzTmp+dVxTmp*dVxTmp+dVyTmp*dVyTmp)/clight/clight)-dErest;
		vdETmp.push_back(dETmp);
		if(lossFlag==0)
		{
			dXTmp=p.at(iTmp).getPosition().getX();
			dYTmp=p.at(iTmp).getPosition().getY();
			dZTmp=p.at(iTmp).getPosition().getZ();
			dBetaAve+=sqrt(dVzTmp*dVzTmp+dVxTmp*dVxTmp+dVyTmp*dVyTmp)/clight;
		  dPxTmp=dVxTmp*dErest*1.0e6/clight/clight*(dETmp/dErest+1);
		  dPyTmp=dVyTmp*dErest*1.0e6/clight/clight*(dETmp/dErest+1);
		  dPzTmp=dVzTmp*dErest*1.0e6/clight/clight*(dETmp/dErest+1);
			p.at(iTmp).setVelocity(dPxTmp,dPyTmp,dPzTmp);
			dXTmp=dXTmp+dVxTmp*(dZAve-dZTmp)/dVzTmp*diagflag;
			dYTmp=dYTmp+dVyTmp*(dZAve-dZTmp)/dVzTmp*diagflag;
			p.at(iTmp).setPosition(dXTmp,dYTmp,dZTmp);
			if(iTmp==1) { dWmin=dETmp;dWmax=dETmp; }
			if(iTmp!=1) 
			{ 
			  if(dETmp<dWmin) dWmin=dETmp;
				if(dETmp>dWmax) dWmax=dETmp;
			}
			dRTmp=sqrt(dXTmp*dXTmp+dYTmp*dYTmp);
			if(fabs(dXTmp)>dXMax)  dXMax=fabs(dXTmp);
			if(fabs(dYTmp)>dYMax)  dYMax=fabs(dYTmp);
			if(dZTmp>dZMax)  dZMax=dZTmp;
			if(dZTmp<dZMin)  dZMin=dZTmp;
			if(dRMax<dRTmp)   dRMax=dRTmp;
			dXAve+=dXTmp;
			dYAve+=dYTmp;
			dRAve+=dRTmp;

			dPxAve+=dPxTmp;
			dPyAve+=dPyTmp;
			dPzAve+=dPzTmp;
			dEnergyAve+=dETmp;
		}
	}
	dXAve/=GoodParticle;
	dYAve/=GoodParticle;
	dRAve/=GoodParticle;
	dPxAve/=GoodParticle;
	dPyAve/=GoodParticle;
	dPzAve/=GoodParticle;
	dEnergyAve=dEnergyAve/GoodParticle;                
	dBetaAve/=GoodParticle;

	//rms值
	dXRms=0;dYRms=0;dZRms=0;dPhiRms=0;
	dXpRms=0;dYpRms=0;dZpRms=0;
	dPxRms=0;dPyRms=0;dPzRms=0;
	dERms=0;
	dXXp=0;dYYp=0;dZZp=0;
	dXPx=0;dYPy=0;dZPz=0;
	double dPhiW=0;
	//for 4D emit
	double dXY,dXPy,dPxPy,dPxY;
	dXY=0;dXPy=0;dXPy=0;dPxPy=0;
	//for 6D emit 
	double dXZ,dXPz,dPxZ,dPxPz,dYZ,dYPz,dPyZ,dPyPz;
	dXZ=0;dXPz=0;dPxZ=0;dPxPz=0;dYZ=0;dYPz=0;dPyZ=0;dPyPz=0;
	
	for(int iTmp=1;iTmp<=particleNumber;iTmp++)
	{
		lossFlag=p.at(iTmp).getLossFlag();
		if(lossFlag==0)
		{
			dXRms+=pow(p.at(iTmp).getPosition().getX()-dXAve,2);
			dYRms+=pow(p.at(iTmp).getPosition().getY()-dYAve,2);
			dZRms+=pow(p.at(iTmp).getPosition().getZ()-dZAve,2);    //20120709
			dPxRms+=pow(p.at(iTmp).getVelocity().getX()-dPxAve,2);
			dPyRms+=pow(p.at(iTmp).getVelocity().getY()-dPyAve,2);
			dPzRms+=pow(p.at(iTmp).getVelocity().getZ()-dPzAve,2);
		//dZpRms+=pow(p.at(iTmp).getVelocity().getZ()-dEnergyAve,2);
			dERms+=pow(vdETmp[iTmp]-dEnergyAve,2); 
			dXPx+=(p.at(iTmp).getPosition().getX()-dXAve)*(p.at(iTmp).getVelocity().getX()-dPxAve);
			dYPy+=(p.at(iTmp).getPosition().getY()-dYAve)*(p.at(iTmp).getVelocity().getY()-dPyAve);
			dZPz+=(p.at(iTmp).getPosition().getZ()-dZAve)*(p.at(iTmp).getVelocity().getZ()-dPzAve);  //20120726
			//for 4D emit
			dXY+=(p.at(iTmp).getPosition().getX()-dXAve)*(p.at(iTmp).getPosition().getY()-dYAve);
			dXPy+=(p.at(iTmp).getPosition().getX()-dXAve)*(p.at(iTmp).getVelocity().getY()-dPyAve);
			dPxY+=(p.at(iTmp).getVelocity().getX()-dPxAve)*(p.at(iTmp).getPosition().getY()-dYAve);
			dPxPy+=(p.at(iTmp).getVelocity().getX()-dPxAve)*(p.at(iTmp).getVelocity().getY()-dPyAve);
			//for 6D emit
			dXZ+=(p.at(iTmp).getPosition().getX()-dXAve)*(p.at(iTmp).getPosition().getZ()-dZAve);
			dXPz+=(p.at(iTmp).getPosition().getX()-dXAve)*(p.at(iTmp).getVelocity().getZ()-dPzAve);
			dPxZ+=(p.at(iTmp).getVelocity().getX()-dPxAve)*(p.at(iTmp).getPosition().getZ()-dZAve);
			dPxPz+=(p.at(iTmp).getVelocity().getX()-dPxAve)*(p.at(iTmp).getVelocity().getZ()-dPzAve);

			dYZ+=(p.at(iTmp).getPosition().getY()-dYAve)*(p.at(iTmp).getPosition().getZ()-dZAve);
			dYPz+=(p.at(iTmp).getPosition().getY()-dYAve)*(p.at(iTmp).getVelocity().getZ()-dPzAve);
			dPyZ+=(p.at(iTmp).getVelocity().getY()-dPyAve)*(p.at(iTmp).getPosition().getZ()-dZAve);
			dPyPz+=(p.at(iTmp).getVelocity().getY()-dPyAve)*(p.at(iTmp).getVelocity().getZ()-dPzAve);

		}
	}
	dXRms=sqrt(dXRms/GoodParticle);
	dYRms=sqrt(dYRms/GoodParticle);
	dZRms=sqrt(dZRms/GoodParticle);
	dPxRms=sqrt(dPxRms/GoodParticle);
	dPyRms=sqrt(dPyRms/GoodParticle);
	dPzRms=sqrt(dPzRms/GoodParticle);
	dERms=sqrt(dERms/GoodParticle);
	dXPx/=GoodParticle;
	dYPy/=GoodParticle;
	dZPz/=GoodParticle;
	//for 4D emit
	dXY/=GoodParticle;
	dPxY/=GoodParticle;
	dXPy/=GoodParticle;
	dPxPy/=GoodParticle;
	//for 6D emit
	dXZ/=GoodParticle;
	dPxZ/=GoodParticle;
	dXPz/=GoodParticle;
	dPxPz/=GoodParticle;

	dYZ/=GoodParticle;
	dPyZ/=GoodParticle;
	dYPz/=GoodParticle;
	dPyPz/=GoodParticle;
	//要计算的2D发射度xxp，yyp，phiW，zzp，zDelt和相应的alph、beta值
	//  Vector3d计算的是xxp，yyp，zzp，单位是cm＊mrad

	double dEmitRmsX,dEmitRmsY,dEmitRmsZ;
	double dBetaRmsX,dBetaRmsY,dBetaRmsZ;
	double dAlphRmsX,dAlphRmsY,dAlphRmsZ;
	dEmitRmsX=pow(dXRms*dXRms*dPxRms*dPxRms-dXPx*dXPx,0.5);
	dEmitRmsY=pow(dYRms*dYRms*dPyRms*dPyRms-dYPy*dYPy,0.5);
	dEmitRmsZ=pow(dZRms*dZRms*dPzRms*dPzRms-dZPz*dZPz,0.5);   //

	CLHEP::HepSymMatrix *matrix_emit=new CLHEP::HepSymMatrix(6,1);

	(*matrix_emit)[0][0]=  dXRms*dXRms*1.0e5;
	(*matrix_emit)[0][1]=         dXPx*1.0e5;
	(*matrix_emit)[0][2]=          dXY*1.0e5;
	(*matrix_emit)[0][3]=         dXPy*1.0e5;

	(*matrix_emit)[1][1]=dPxRms*dPxRms*1.0e5;
	(*matrix_emit)[1][2]=         dPxY*1.0e5;
	(*matrix_emit)[1][3]=        dPxPy*1.0e5;

	(*matrix_emit)[2][2]=  dYRms*dYRms*1.0e5;
	(*matrix_emit)[2][3]=         dYPy*1.0e5;

	(*matrix_emit)[3][3]=dPyRms*dPyRms*1.0e5;

  dEmit4D=matrix_emit->determinant();
	dEmit4D=sqrt(dEmit4D); //nunormalized

  //for emit 6D
	(*matrix_emit)[0][4]=  dXZ*1.0e5;
	(*matrix_emit)[0][5]= dXPz*1.0e5;
	(*matrix_emit)[1][4]= dPxZ*1.0e5;
	(*matrix_emit)[1][5]=dPxPz*1.0e5;
	(*matrix_emit)[2][4]=  dYZ*1.0e5;
	(*matrix_emit)[2][5]= dYPz*1.0e5;
	(*matrix_emit)[3][4]= dPyZ*1.0e5;
	(*matrix_emit)[3][5]=dPyPz*1.0e5;
	
	(*matrix_emit)[4][4]=  dZRms*dZRms*1.0e5;
	(*matrix_emit)[4][5]=         dZPz*1.0e5;
	(*matrix_emit)[5][5]=dPzRms*dPzRms*1.0e5;

  dEmit6D=matrix_emit->determinant();
	dEmit6D=sqrt(dEmit6D);

	dAlphRmsX=-dXPx/dEmitRmsX;                   
	dAlphRmsY=-dYPy/dEmitRmsY;
	dAlphRmsZ=-dZPz/dEmitRmsZ;
	dBetaRmsX=dXRms*dXRms/dEmitRmsX;     //cm/mrad  with unnormalized emittance
	dBetaRmsY=dYRms*dYRms/dEmitRmsY;     //cm/mrad
	dBetaRmsZ=dZRms*dZRms/dEmitRmsZ;           //cm/mrad

//emit/mc*1.0e5  //unit cm*mrad
	dEmitRmsX/=(dErest*10.0/clight);                 //or dPAve?
	dEmitRmsY/=(dErest*10.0/clight);
	dEmitRmsZ/=(dErest*10.0/clight);

  double dGx,dGy,dGz;
	dGx=(1.0+dAlphRmsX*dAlphRmsX)/dBetaRmsX;
	dGy=(1.0+dAlphRmsY*dAlphRmsY)/dBetaRmsY;
	dGz=(1.0+dAlphRmsZ*dAlphRmsZ)/dBetaRmsZ;
  dXpRms=sqrt(dGx*dEmitRmsX);
  dYpRms=sqrt(dGy*dEmitRmsY);
  dZpRms=sqrt(dGz*dEmitRmsZ);

	//dEmitRmsX*=(dBetaAve*(dEnergyAve/dErest+1.0));       //cm*mrad
	//dEmitRmsY*=(dBetaAve*(dEnergyAve/dErest+1.0));       //cm*mrad
	//dEmitRmsZ*=(dBetaAve*pow((dEnergyAve/dErest+1.0),3));       //cm*mrad
	EmitRms->setCVector3d(dEmitRmsX,dEmitRmsY,dEmitRmsZ); 

	RmsSize->setCVector3d(dXRms,dYRms,dZRms);
	//RmsSizep->setCVector3d(dXpRms,dYpRms,dZpRms);
	RmsSizep->setCVector3d(dPxRms,dPyRms,dPzRms);
	AveSize->setCVector3d(dXAve,dYAve,dZAve);
	AveSizep->setCVector3d(dPxAve,dPyAve,dPzAve);
	alph->setCVector3d(dAlphRmsX,dAlphRmsY,dAlphRmsZ);
	beta->setCVector3d(dBetaRmsX,dBetaRmsY,dBetaRmsZ);

	//----change coordinates to original
	for(int iTmp=1;iTmp<=particleNumber;iTmp++)
	{
		lossFlag=p.at(iTmp).getLossFlag();
		if(lossFlag==0)
		{
			dETmp=vdETmp[iTmp];
			dVxTmp=p.at(iTmp).getVelocity().getX()/dErest/1.0e6*clight*clight/(dETmp/dErest+1);
			dVyTmp=p.at(iTmp).getVelocity().getY()/dErest/1.0e6*clight*clight/(dETmp/dErest+1);
			dVzTmp=p.at(iTmp).getVelocity().getZ()/dErest/1.0e6*clight*clight/(dETmp/dErest+1);
			p.at(iTmp).setVelocity(dVxTmp,dVyTmp,dVzTmp);
			dXTmp=p.at(iTmp).getPosition().getX();
			dYTmp=p.at(iTmp).getPosition().getY();
			dZTmp=p.at(iTmp).getPosition().getZ();
			dXTmp=dXTmp-dVxTmp*(dZAve-dZTmp)/dVzTmp*diagflag;
			dYTmp=dYTmp-dVyTmp*(dZAve-dZTmp)/dVzTmp*diagflag;
			p.at(iTmp).setPosition(dXTmp,dYTmp,dZTmp);
		}
	}
	delete matrix_emit;
}
void CBeam::outBeam(string _str)
{
	ofstream of(_str.c_str());
	of<<"   XRms    YRms      PhiRms    ERms      Xmax     Ymax     XAve       YAve      PhiAve  dEnergyAve"<<endl;
	of<<dXRms*100<<" "<<dYRms*100<<" "<<dPhiRms<<" "<<dERms<<" "<<dXMax*100<<" "
		<<dYMax*100<<" "<<dXAve*100<<" "<<dYAve*100<<" "
		<<p.at(0).getPosition().getZ()*360/dBetaS/clight*dFreq<<" "<<dEnergyAve<<endl;
	of.close();
}
//double CBeam::getEmitRmsZ() const { return dEmitRmsZ; }
void CBeam::setEnergyS(double _dEnergyS) 
{
	dEnergyS=_dEnergyS;
	dBetaS=dEnergyS/dErest+1.0;
	dBetaS=sqrt(1-1/dBetaS/dBetaS);
	p.at(0).setVelocity(0,0,dBetaS*clight);
}
void CBeam::DisTest()
{
ofstream of("CBeam_Dis");
for(int i=0;i<p.size();i++)
of<<p.at(i).getPosition().getX()<<" "
  <<p.at(i).getPosition().getY()<<" "
  <<p.at(i).getPosition().getZ()<<" "
  <<p.at(i).getVelocity().getX()<<" "
  <<p.at(i).getVelocity().getY()<<" "
  <<p.at(i).getVelocity().getZ()<<"\n "; 
	of.close();
	of.open("CBeam_Dis_Test");
for(int i=0;i<p.size();i++)
of<<p.at(i).getPosition().getX()<<" "
  <<p.at(i).getPosition().getY()<<endl;
	of.close();
}

